package openglsuperbible.glutils;
import org.lwjgl.util.vector.Matrix3f;
import org.lwjgl.util.vector.Matrix4f;
import org.lwjgl.util.vector.Vector3f;
import static openglsuperbible.glutils.MatrixUtils.*;
import static org.lwjgl.opengl.GL11.*;

/**
 *
 * @author Matthew
 */
public class GLFrame {
    private  Vector3f  origin;	// Where am I?
    private  Vector3f  forward;	// Where am I going?
    private  Vector3f  up;      // Which way is up?

    public GLFrame() {
        
        origin = new Vector3f(0, 0, 0);
        forward = new Vector3f(0, 0, -1);
        up = new Vector3f(0, 1, 0);
        
    }

 public Vector3f getRight() {
        return Vector3f.cross(up, forward, null);
    }

    public void translateWorld(float x, float y, float z) {
        origin.setX(origin.getX() + x);
        origin.setY(origin.getX() + y);
        origin.setZ(origin.getX() + z);
    }

    public void translateLocal(float x, float y, float z) {
        moveForward(z);
        moveUp(y);
        moveRight(x);
    }

    public void moveForward(float delta) {
        origin.setX(origin.getX() + forward.getX() * delta);
        origin.setY(origin.getY() + forward.getY() * delta);
        origin.setZ(origin.getZ() + forward.getZ() * delta);
    }

    public void moveUp(float delta) {
        origin.setX(origin.getX() + up.getX() * delta);
        origin.setY(origin.getY() + up.getY() * delta);
        origin.setZ(origin.getZ() + up.getZ() * delta);
    }

    public void moveRight(float delta) {
        Vector3f right = getRight();
        origin.setX(origin.getX() + right.getX() * delta);
        origin.setY(origin.getY() + right.getY() * delta);
        origin.setZ(origin.getZ() + right.getZ() * delta);
    }


//    public static Matrix4f fromAxesAndOrigin(Vector3f x, Vector3f y, Vector3f z, Vector3f origin, Matrix4f m) {
//        if (m == null)
//            m = new Matrix4f();
//
//        m.setIdentity();
//
//        if (origin != null) {
//            m.m30 = origin.getX();
//            m.m31 = origin.getY();
//            m.m32 = origin.getZ();
//        }
//
//        return m;
//    }


    public Matrix4f getMatrix( boolean rotationOnly) {
        Matrix4f m = new Matrix4f();

        Vector3f right = getRight();
        m.setIdentity();
        
        m.m00 = right.getX();
        m.m01 = right.getY();
        m.m02 = right.getZ();

        m.m10 = up.getX();
        m.m11 = up.getY();
        m.m12 = up.getZ();

        m.m20 = forward.getX();
        m.m21 = forward.getY();
        m.m22 = forward.getZ();

        if (!rotationOnly) {
            m.m30 = origin.getX();
            m.m31 = origin.getY();
            m.m32 = origin.getZ();
        }
        return m;
    }


    public Matrix4f getMatrix() {

        return getMatrix( false);
    }
    
    public float[] getMatrixf() {

        return this.matrix4fToFloatArray(getMatrix( false));
    }
    
    public float[] getMatrixf( boolean rotationOnly) {

        return this.matrix4fToFloatArray(getMatrix( rotationOnly));
    }
    
   public float[] getCameraMatrix( ) {
        Matrix4f m = new Matrix4f();

        m.setIdentity();
        Vector3f z = forward.negate(null);
        Vector3f x = Vector3f.cross(up, z, null);

        m.m00 = x.getX();
        m.m01 = x.getY();
        m.m02 = x.getZ();
        
        m.m10 = up.getX();
        m.m11 = up.getY();
        m.m12 = up.getZ();

        m.m20 = z.getX();
        m.m21 = z.getY();
        m.m22 = z.getZ();
        
        float[] trans = new float[16];
        float[] M = new float[16];
        Math3D.translationMatrix44f(trans, -origin.getX(), -origin.getY(), -origin.getY());
        Math3D.matrixMultiply44(M, matrix4fToFloatArray(m), trans);
        
        return M;
    }
    

    
    public Matrix3f getRotationMatrix(Matrix3f m) {
        if (m == null)
            m = new Matrix3f();

        Vector3f right = getRight();
        m.setIdentity();

        m.m00 = right.getX();
        m.m01 = right.getY();
        m.m02 = right.getZ();

        m.m10 = up.getX();
        m.m11 = up.getY();
        m.m12 = up.getZ();

        m.m20 = forward.getX();
        m.m21 = forward.getY();
        m.m22 = forward.getZ();

        return m;
    }

    public Matrix3f getRotationMatrix() {
        Matrix3f ignore = null;
        return getRotationMatrix(ignore);
    }


    public Matrix4f getCameraOrientation(Matrix4f m) {
        if (m == null)
            m = new Matrix4f();

        m.setIdentity();

        Vector3f z = forward.negate(null);
        Vector3f x = Vector3f.cross(up, z, null);

        m.m00 = x.getX();
        m.m01 = x.getY();
        m.m02 = x.getZ();

        m.m10 = up.getX();
        m.m11 = up.getY();
        m.m12 = up.getZ();

        m.m20 = z.getX();
        m.m21 = z.getY();
        m.m22 = z.getZ();

        return m;
    }

    public float[] getCameraOrientationf() {
        return this.matrix4fToFloatArray(getCameraOrientation());
    }
    
    public Matrix4f getCameraOrientation() {
        Matrix4f ignored = null;
        return getCameraOrientation(ignored);
    }

    public void applyCameraTransform(boolean rotateOnly) {
        MatrixUtils.glMultMatrix(getCameraOrientation());
        if (!rotateOnly) {
            glTranslatef(-origin.getX(), -origin.getY(), -origin.getZ());
        }
    }

    public void applyCameraTransform() {
        applyCameraTransform(false);
    }

    public void applyActorTransform(boolean rotateOnly) {
        glMultMatrix(getMatrix(rotateOnly));
    }

    public void applyActorTransform() {
        applyActorTransform(false);
    }

    public void rotateLocalX(float angle) {
        Matrix3f rot = makeRotationmatrix(angle, getRight());
        Matrix3f.transform(rot, forward, forward);
        Matrix3f.transform(rot, up, up);
    }

    public void rotateLocalY(float angle) {
        Matrix3f rot = makeRotationmatrix(angle, up);
        Matrix3f.transform(rot, forward, forward);
    }

    public void rotateLocalZ(float angle) {
        Matrix3f rot = makeRotationmatrix(angle, forward);
        Matrix3f.transform(rot, up, up);
    }

//    public void normalize() {
//        Vector3f right = getRight();
//        // recalculate forward
//        Vector3f.cross(right, up, forward);
//        up = (Vector3f) up.normalise();
//        forward = (Vector3f) forward.normalise();
//    }

    public void rotateWorld(float angle, float x, float y, float z) {
        Matrix3f m = makeRotationMatrix(angle, x, y, z);
        Matrix3f.transform(m, up, up);
        Matrix3f.transform(m, forward, forward);
    }

    public void rotateLocal(float angle, float x, float y, float z) {
        Vector3f local = new Vector3f(x, y, z);
        Vector3f world = localToWorld(local);
        rotateWorld(angle, world.getX(), world.getY(), world.getZ());
    }

    public Vector3f localToWorld(Vector3f local, Vector3f world) {
        if (world == null)
            world = new Vector3f();
        Matrix3f m = getRotationMatrix();
        Matrix3f.transform(m, local, world);
        Vector3f.add(world, origin, world);
        return world;
    }

    public Vector3f localToWorld(Vector3f local) {
        Vector3f ignored = null;
        return localToWorld(local, ignored);
    }

    public Vector3f worldToLocal(Vector3f world, Vector3f local) {
        if (local == null)
            local = new Vector3f();

        Vector3f v = Vector3f.sub(world, origin, null);

        Matrix3f m = getRotationMatrix();
        m.invert();
        Matrix3f.transform(m, v, local);
        return local;
    }

    public Vector3f worldToLocal(Vector3f world) {
        Vector3f ignored = null;
        return worldToLocal(world, ignored);
    }


//    public Vector3f transformPoint(Vector3f src, Vector3f dest) {
//        if (dest == null)
//            dest = new Vector3f();
//
//        Matrix4f m = getMatrix();
//        float sx = src.getX();
//        float sy = src.getY();
//        float sz = src.getZ();
//
//        // Uhhhh.... ?
//        dest.setX(m.m00 * sx + m.m01 * sy + m.m02 * sz + m.m03);
//        dest.setY(m.m10 * sx + m.m11 * sy + m.m12 * sz + m.m13);
//        dest.setZ(m.m20 * sx + m.m21 * sy + m.m22 * sz + m.m23);
//
//        return dest;
//    }
//
//    // Nor this one either
//    public Vector3f rotateVector(Vector3f src, Vector3f dest) {
//        if (dest == null)
//            dest = new Vector3f();
//
//        Matrix4f m = getMatrix();
//        float sx = src.getX();
//        float sy = src.getY();
//        float sz = src.getZ();
//
//        // Again with the transposed matrix...
//        dest.setX(m.m00 * sx + m.m01 * sy + m.m02 * sz);
//        dest.setY(m.m10 * sx + m.m11 * sy + m.m12 * sz);
//        dest.setZ(m.m20 * sx + m.m21 * sy + m.m22 * sz);
//
//        return dest;
//    }

   

    public Vector3f getOrigin() {
        return origin;
    }

    public void setOrigin(float x,float y, float z) {      
        
        origin.setX(x);
        origin.setY(y);
        origin.setZ(z);
    }
    public void setOrigin(Vector3f origin) {
        this.origin = origin;
    }

    public Vector3f getForward() {
        return forward;
    }

    public void setForward(Vector3f forward) {
        this.forward = forward;
    }

    public Vector3f getUp() {
        return up;
    }

    public void setUp(Vector3f up) {
        this.up = up;
    }


//    public void setvOrigin(float[] vOrigin) {
//        this.vOrigin = vOrigin;
//    }
//
//    public void setvOrigin(float x, float y, float z) {
//        vOrigin[0] = x; vOrigin[1] = y; vOrigin[2] = z;
//    }
//
//    public float[] getvOrigin() {
//        return vOrigin;
//    }
//        
//    public float getOriginX() { return vOrigin[0]; }
//    public float getOriginY() { return vOrigin[1]; } 
//    public float getOriginZ() { return vOrigin[2]; }   
//
//    
//    
//    public void setvForwardVector(float[] vForward) {
//        this.vForward = vForward;
//    }
//    
//    public void setvForwardVector(float x, float y, float z) {
//         vForward[0] = x; vForward[1] = y; vForward[2] = z;
//    }
//
//    public float[] getForwardVector() {
//        return vForward;
//    }
//     
//    
//    public void setvUpVector(float[] vUp) {
//        this.vUp = vUp;
//    }
//    
//    public void setvUpVector(float x, float y, float z) {
//        vUp[0] = x; vUp[1] = y; vUp[2] = z;
//    }
//
//    public float[] getUpVector() {
//        return vUp;
//    }
//    
//    public float[] getZAxis(){ return getForwardVector(); }
//    public float[] getYAxis() { return getUpVector(); }
//    public float[] getXAxis() { return Math3D.crossProduct3( vUp, vForward); }
//    
//    
//    /**
//     * Translate along orthonormal axis world
//     */
//    public void translateWorld(float x, float y, float z)
//                    { vOrigin[0] += x; vOrigin[1] += y; vOrigin[2] += z; }
//    /**
//     * Translate along orthonormal axis local
//     */
//    public void translateLocal(float x, float y, float z)
//    { 
//        moveForward(z); moveUp(y); moveRight(x);
//    }
//
//    /**
//     * Move Forward (along Z axis)
//     * @param fDelta 
//     */
//    public void moveForward(float fDelta)
//    {
//        // Move along direction of front direction
//            vOrigin[0] += vForward[0] * fDelta;
//            vOrigin[1] += vForward[1] * fDelta;
//            vOrigin[2] += vForward[2] * fDelta;
//    }
//
//    /** 
//     * Move along Y axis
//     * @param fDelta 
//     */
//    public void moveUp(float fDelta)
//    {
//        // Move along direction of up direction
//            vOrigin[0] += vUp[0] * fDelta;
//            vOrigin[1] += vUp[1] * fDelta;
//            vOrigin[2] += vUp[2] * fDelta;
//    }
//
//    /**
//     * Move along X axis
//     * @param fDelta 
//     */
//    public void moveRight(float fDelta)
//    {
//            // Move along direction of right vector
//            float[] vCross = Math3D.crossProduct3(vUp, vForward);	
//
//            vOrigin[0] += vCross[0] * fDelta;
//            vOrigin[1] += vCross[1] * fDelta;
//            vOrigin[2] += vCross[2] * fDelta;
//    }
//        
//    public float[] getMatrix( )
//    {
//        return getMatrix(false);
//    }
//        
//    /**
//     * Just assemble the matrix
//     */
//    public float[] getMatrix(boolean bRotationOnly)
//    {
//	// Calculate the right side (x) vector, drop it right into the matrix
//            float[] vXAxis = Math3D.crossProduct3(vUp, vForward);
//            float[] matrix = new float[16];
//            
//
//	   // Set matrix column does not fill in the fourth value...
//            Math3D.setMatrixColumn44(matrix, vXAxis, 1);
//            matrix[3] = 0.0f;
//           
//            // Y Column
//	    Math3D.setMatrixColumn44(matrix, vUp, 2);
//            matrix[7] = 0.0f;       
//                                    
//            // Z Column
//	    Math3D.setMatrixColumn44(matrix, vForward, 3);
//            matrix[11] = 0.0f;
//
//            // Translation (already done)
//            if(bRotationOnly == true)
//                    {
//                    matrix[12] = 0.0f;
//                    matrix[13] = 0.0f;
//                    matrix[14] = 0.0f;
//                    }
//            else
//                    Math3D.setMatrixColumn44(matrix, vOrigin, 3);
//
//            matrix[15] = 1.0f;
//            return matrix;
//    }    
//    
//    public float[] getCameraMatrix()
//    {
//        return getCameraMatrix( false);
//    }
//    
//    /**
//     * Assemble the camera matrix
//     */
//    public float[] getCameraMatrix( boolean bRotationOnly)
//    {
//        float[] m = new float[16];
//        float[] x;
//        float[] z = new float[3];
//
//        // Make rotation matrix
//        // Z vector is reversed
//        z[0] = -vForward[0];
//        z[1] = -vForward[1];
//        z[2] = -vForward[2];
//
//        // X vector = Y cross Z 
//        x = Math3D.crossProduct3(vUp, z);
//
//        if(bRotationOnly)
//        {
//            return m;
//        }
//        else{
//            
//            // Apply translation too
//            float[] trans = new float[16];                    
//            float[] M = new float[16] ;
//            
//            Math3D.translationMatrix44f(trans, -vOrigin[0], -vOrigin[1], -vOrigin[2]);  
//
//            Math3D.matrixMultiply44(M, m, trans);
//
//            // Copy result back into m
//            System.arraycopy(m,0,M,0,16);
//            return m;
//        }
//    }
//        
//        /**
//         * Rotate around local Y
//         */
//        public void rotateLocalY(float fAngle)
//        {
//            float[] rotMat = new float[3];  
//
//            // Just Rotate around the up vector
//            // Create a rotation matrix around my Up (Y) vector
//            Math3D.rotationMatrix44(rotMat, fAngle,
//                vUp[0], vUp[1], vUp[2]);
//
//            float[] newVect = new float[16]; 
//
//            // Rotate forward pointing vector (inlined 3x3 transform)
//            newVect[0] = rotMat[0] * vForward[0] + rotMat[4] * vForward[1] + rotMat[8] *  vForward[2];	
//            newVect[1] = rotMat[1] * vForward[0] + rotMat[5] * vForward[1] + rotMat[9] *  vForward[2];	
//            newVect[2] = rotMat[2] * vForward[0] + rotMat[6] * vForward[1] + rotMat[10] * vForward[2];	
//            vForward = newVect;
//        }
//
//
//	/**
//         * Rotate around local Z
//         */
//        public void rotateLocalZ(float fAngle)
//        {
//            float[] rotMat = new float[3];  
//
//            // Only the up vector needs to be rotated
//            Math3D.rotationMatrix44(rotMat, fAngle,vForward[0], vForward[1], vForward[2]);
//
//            float[] newVect = new float[3];
//            newVect[0] = rotMat[0] * vUp[0] + rotMat[4] * vUp[1] + rotMat[8] *  vUp[2];	
//            newVect[1] = rotMat[1] * vUp[0] + rotMat[5] * vUp[1] + rotMat[9] *  vUp[2];	
//            newVect[2] = rotMat[2] * vUp[0] + rotMat[6] * vUp[1] + rotMat[10] * vUp[2];	
//            vUp = newVect;
//        }
//
//        public void rotateLocalX(float fAngle)
//        {
//            float[] rotMat = new float[16];  
//            float[]  localX;
//            
//            // Get the local X axis
//            localX = Math3D.crossProduct3(vUp, vForward);
//
//            // Make a Rotation Matrix
//            Math3D.rotationMatrix44(rotMat, fAngle, localX[0], localX[1], localX[2]);
//
//            // Rotate Y, and Z           
//            vUp =  Math3D.rotateVector( vUp, rotMat);            
//            vForward = Math3D.rotateVector( vForward, rotMat);
//        }
//
//
//        /**
//         * Reset axes to make sure they are orthonormal. This should be called on occasion
//         * if the matrix is long-lived and frequently transformed.
//         */
//        public void Normalize()
//        {
//                // Calculate cross product of up and forward vectors
//            float[] vCross = Math3D.crossProduct3( vUp, vForward);
//                // Use result to recalculate forward vector
//            vForward = Math3D.crossProduct3(vCross, vUp);
//
//            // Also check for unit length...
//            Math3D.normalizeVector3(vUp);
//            Math3D.normalizeVector3(vForward);
//        }
//
//
//        /**
//         * Rotate in world coordinates...
//         */
//       public  void rotateWorld(float fAngle, float x, float y, float z)
//        {
//            float[] rotMat = new float[16];
//
//            // Create the Rotation matrix
//            Math3D.rotationMatrix44( rotMat,fAngle, x, y, z);
//
//            float[] newVect = new float[3];
//
//            // Transform the up axis (inlined 3x3 rotation)
//            newVect[0] = rotMat[0] * vUp[0] + rotMat[4] * vUp[1] + rotMat[8] *  vUp[2];	
//            newVect[1] = rotMat[1] * vUp[0] + rotMat[5] * vUp[1] + rotMat[9] *  vUp[2];	
//            newVect[2] = rotMat[2] * vUp[0] + rotMat[6] * vUp[1] + rotMat[10] * vUp[2];	
//            vUp = newVect;
//
//            // Transform the forward axis
//            newVect[0] = rotMat[0] * vForward[0] + rotMat[4] * vForward[1] + rotMat[8] *  vForward[2];	
//            newVect[1] = rotMat[1] * vForward[0] + rotMat[5] * vForward[1] + rotMat[9] *  vForward[2];	
//            newVect[2] = rotMat[2] * vForward[0] + rotMat[6] * vForward[1] + rotMat[10] * vForward[2];	
//            vForward = newVect;
//        }
//
//
//        /**
//         * Rotate around a local axis
//         */
//        public void rotateLocal(float fAngle, float x, float y, float z) 
//        {
//            float[] vWorldVect = new float[3];
//            float[] vLocalVect = {x,y,z};    
//
//            localToWorld(vLocalVect, vWorldVect, true);
//            rotateWorld(fAngle, vWorldVect[0], vWorldVect[1], vWorldVect[2]);
//        }
//    
//        
//        public void localToWorld(float[] vLocal, float[] vWorld)
//        {
//            localToWorld( vLocal, vWorld, false);
//        }
//
//        /**
//         * Convert Coordinate Systems
//         * This is pretty much, do the transformation represented by the rotation
//         * and position on the point
//	 * Is it better to stick to the convention that the destination always comes
//	 * first, or use the conventions that "sounds" like the function...
//         */
//        public void localToWorld(float[] vLocal, float[] vWorld, boolean bRotOnly)
//        {
//             // Create the rotation matrix based on the vectors
//            float[] rotMat = getMatrix(true);                 
//
//            // Do the rotation (inline it, and remove 4th column...)
//            vWorld[0] = rotMat[0] * vLocal[0] + rotMat[4] * vLocal[1] + rotMat[8] *  vLocal[2];	
//            vWorld[1] = rotMat[1] * vLocal[0] + rotMat[5] * vLocal[1] + rotMat[9] *  vLocal[2];	
//            vWorld[2] = rotMat[2] * vLocal[0] + rotMat[6] * vLocal[1] + rotMat[10] * vLocal[2];	
//
//            // Translate the point
//            if(!bRotOnly) {
//                vWorld[0] += vOrigin[0];
//                vWorld[1] += vOrigin[1];
//                vWorld[2] += vOrigin[2];
//            }
//        }
//
//	/**
//         * Change world coordinates into "local" coordinates
//         */
//        public void worldToLocal( float[] vWorld, float[] vLocal)
//        {
//			////////////////////////////////////////////////
//            // Translate the origin
//	    float[] vNewWorld = new float[3];
//            vNewWorld[0] = vWorld[0] - vOrigin[0];
//            vNewWorld[1] = vWorld[1] - vOrigin[1];
//            vNewWorld[2] = vWorld[2] - vOrigin[2];
//
//            // Create the rotation matrix based on the vectors
//	    float[] rotMat = getMatrix(true);
//            float[] invMat = new float[16];
//	    
//
//	    // Do the rotation based on inverted matrix
//            Math3D.invertMatrix44(invMat, rotMat);
//
//            vLocal[0] = invMat[0] * vNewWorld[0] + invMat[4] * vNewWorld[1] + invMat[8] *  vNewWorld[2];	
//            vLocal[1] = invMat[1] * vNewWorld[0] + invMat[5] * vNewWorld[1] + invMat[9] *  vNewWorld[2];	
//            vLocal[2] = invMat[2] * vNewWorld[0] + invMat[6] * vNewWorld[1] + invMat[10] * vNewWorld[2];	
//        }
//        
//        /**
//         * Transform a point by frame matrix
//         */
//        public void transformPoint(float[] vPointSrc, float[] vPointDst)
//        {
//            float[] m = getMatrix(false);  
//              // Rotate and translate
//            vPointDst[0] = m[0] * vPointSrc[0] + m[4] * vPointSrc[1] + m[8] *  vPointSrc[2] + m[12];// * v[3];	 
//            vPointDst[1] = m[1] * vPointSrc[0] + m[5] * vPointSrc[1] + m[9] *  vPointSrc[2] + m[13];// * v[3];	
//            vPointDst[2] = m[2] * vPointSrc[0] + m[6] * vPointSrc[1] + m[10] * vPointSrc[2] + m[14];// * v[3];	
//        }
//        
//        /**
//         * Rotate a vector by frame matrix
//         */
//        public void rotateVector(float[] vVectorSrc, float[] vVectorDst)
//        {
//            float[] m = getMatrix( true);    // Rotate only            
//            
//            vVectorDst[0] = m[0] * vVectorSrc[0] + m[4] * vVectorSrc[1] + m[8] *  vVectorSrc[2];	 
//            vVectorDst[1] = m[1] * vVectorSrc[0] + m[5] * vVectorSrc[1] + m[9] *  vVectorSrc[2];	
//            vVectorDst[2] = m[2] * vVectorSrc[0] + m[6] * vVectorSrc[1] + m[10] * vVectorSrc[2];	
//        }            
//        
    /**
     * Returns a float array of the given 4x4 matrix
     * @param matrix
     * @return 
     */
    private float[] matrix4fToFloatArray(Matrix4f matrix)
    {
        float[] array = new float[16];
       
            array[0] = matrix.m00;
            array[1] = matrix.m01;
            array[2] = matrix.m02;
            array[3] = matrix.m03;
            array[4] = matrix.m10;
            array[5] = matrix.m11;
            array[6] = matrix.m12;
            array[7] = matrix.m13;
            array[8] = matrix.m20;
            array[9] = matrix.m21;
            array[10] = matrix.m22;
            array[11] = matrix.m23;
            array[12] = matrix.m30;
            array[13] = matrix.m31;
            array[14] = matrix.m32;
            array[15] = matrix.m33;
        
        return array;
    }
    
}
